#ifndef ROS_KOALA_H
#define ROS_KOALA_H

#include <string>
#include <iostream>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <std_srvs/Empty.h>
#include <koala_kinematics/Int_Bool.h>
#include <koala_kinematics/Empty_Int.h>
#include <kRobot.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>


class Koala {

    public:

        Koala();
        ~Koala() {}


        void get_left_wheel_counter();
        void get_right_wheel_counter();
        void publish_odometry();



    private:

        kRobot robot;
        std::string name;
        ros::NodeHandle nh;

        ros::Time current_time;
        ros::Time last_time;
        int old_encoder_left;
        int old_encoder_right;

        float lastTrans;
        float lastRot;

        float oldx;
        float oldy;
        float oldth;

        float x;
        float y;
        float th;

        float R;
        float w;

        bool initialized;

        bool turn_angle(koala_kinematics::Int_Bool::Request &in,
                        koala_kinematics::Int_Bool::Response &out);

        bool drive_distance(koala_kinematics::Int_Bool::Request &in,
                            koala_kinematics::Int_Bool::Response &out);

        bool set_wheel_speed(koala_kinematics::Int_Bool::Request &in,
                             koala_kinematics::Int_Bool::Response &out);

        bool set_wheel_acceleration(koala_kinematics::Int_Bool::Request &in,
                                    koala_kinematics::Int_Bool::Response &out);

        bool get_wheel_speed(koala_kinematics::Empty_Int::Request &in,
                             koala_kinematics::Empty_Int::Response &out);

        bool get_wheel_acceleration(koala_kinematics::Empty_Int::Request &in,
                                    koala_kinematics::Empty_Int::Response &out);

        void drive_speed(const geometry_msgs::Twist::ConstPtr &msg);

        void drive_distance_pub(const std_msgs::Int32::ConstPtr &msg);

        void turn_angle_pub(const std_msgs::Int32::ConstPtr &msg);

        ros::ServiceServer koala_turn_angle;
        ros::ServiceServer koala_drive_distance;
        ros::ServiceServer koala_get_wheel_speed;
        ros::ServiceServer koala_get_wheel_acceleration;
        ros::Publisher	   koala_left_wheel_count;
        ros::Publisher	   koala_right_wheel_count;
        ros::Subscriber    koala_drive_speed;
        tf::TransformBroadcaster broadcaster;
        ros::Publisher           odom_pub;
        ros::Subscriber    koala_drive_distance_sub;
        ros::Subscriber    koala_turn_angle_sub;


};


#endif // ROS_KOALA_H
